﻿// main.cpp
#include "Matrix.h"
#include "Constant.h"
#include "CoordSys.h"
#include "TimeSys.h"
#include "ReadN.h"
#include "ReadO.h"
#include "GNSSSatPos.h"
#include "IonosphereCorrection.h"
#include "TroposphereCorrection.h"
#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>

using namespace std;

/*
单点定位数据处理全过程：
1. 初始化：创建时间系统、坐标系统、文件读取器、卫星位置计算器等对象
2. 读取导航文件(N文件)：获取卫星星历数据
3. 读取观测文件(O文件)：获取接收机观测数据
4. 对每个历元进行处理：
   a. 转换观测时间为统一时间系统
   b. 对每颗可见卫星：
      i.   计算卫星位置
      ii.  计算卫星到接收机的几何距离
      iii. 应用误差改正（电离层、对流层、卫星钟差）
      iv.  构建观测方程
   c. 使用最小二乘法求解接收机位置和接收机钟差
   d. 迭代计算直至收敛
   e. 输出定位结果
5. 释放资源
*/

int main() {
    // 初始化系统
    BDSTime bdsTimeSys; // 使用BDST时间系统
    WGS84 wgs84;        // 使用WGS84坐标系统
    ReadN navReader("C:\\1234560660442.17N");
    ReadO obsReader("C:\\1234560660442.17O");

    // 读取导航文件和观测文件
    if (!navReader.parseFile() || !obsReader.parse_file()) {
        cerr << "文件读取失败！" << endl;
        return 1;
    }

    // 获取观测数据
    vector<obsEpoch>* obsData = obsReader.get_obs();
    if (obsData->empty()) {
        cerr << "没有观测数据！" << endl;
        return 1;
    }

    // 创建卫星位置计算器
    BDSSatPos satPosCalc(&bdsTimeSys, &navReader);

    // 初始化接收机位置和钟差（初始值设为0）
    XYZ recPos = { 0, 0, 0 };
    double recClockBias = 0.0;
    const double SPEED_OF_LIGHT = 299792458.0; // 光速

    // 遍历每个历元
    for (auto& epoch : *obsData) {
        // 转换观测时间为BaseT格式
        BaseT obsTime = bdsTimeSys.str2baseT(epoch.time);

        // 初始化最小二乘矩阵
        int numSats = epoch.listObsSat.size();
        Matrix designMat(numSats, 4);  // 设计矩阵
        Matrix obsVector(numSats, 1);  // 观测向量
        Matrix weightMat(numSats, numSats); // 权重矩阵（初始为单位矩阵）

        // 初始化权重矩阵为单位矩阵
        for (int i = 0; i < numSats; i++) {
            weightMat[i][i] = 1.0;
        }

        // 对每颗卫星进行处理
        for (int i = 0; i < numSats; i++) {
            const obsSat& sat = epoch.listObsSat[i];

            // 获取伪距观测值（假设第一个观测值是伪距）
            if (sat.values.empty()) continue;
            double pseudoRange = sat.values[0];

            // 计算卫星位置
            XYZ satPos = satPosCalc.get_satPos(sat.satID, obsTime);

            // 计算几何距离
            double dx = satPos.x - recPos.x;
            double dy = satPos.y - recPos.y;
            double dz = satPos.z - recPos.z;
            double geoDist = sqrt(dx * dx + dy * dy + dz * dz);

            // 计算方向余弦
            double ax = dx / geoDist;
            double ay = dy / geoDist;
            double az = dz / geoDist;

            // 构建设计矩阵
            designMat[i][0] = ax;
            designMat[i][1] = ay;
            designMat[i][2] = az;
            designMat[i][3] = 1.0; // 钟差项

            // 计算高度角和方位角（用于误差改正）
            ENU enu = wgs84.XYZ2ENU(recPos, satPos);
            double elev = atan2(enu.u, sqrt(enu.e * enu.e + enu.n * enu.n));

            // 应用误差改正
            double correction = 0.0;

            // 电离层延迟改正
            IonosphereCorrection ionoCorr;
            correction += ionoCorr.computeDelay(recPos, satPos, obsTime);

            // 对流层延迟改正
            TroposphereCorrection tropoCorr;
            correction += tropoCorr.calculateDelay(recPos, satPos);

            // 应用改正后的伪距
            double correctedPR = pseudoRange + correction;

            // 构建观测向量
            obsVector[i][0] = correctedPR - geoDist - recClockBias * SPEED_OF_LIGHT;
        }

        // 最小二乘解算：ΔX = (AᵀWA)⁻¹AᵀWΔP
        Matrix A_T = designMat.transpose();
        Matrix N = A_T * weightMat * designMat;
        Matrix U = A_T * weightMat * obsVector;

        // 求解法方程
        Matrix deltaX = N.inverse() * U;

        // 更新接收机位置和钟差
        recPos.x += deltaX[0][0];
        recPos.y += deltaX[1][0];
        recPos.z += deltaX[2][0];
        recClockBias += deltaX[3][0] / SPEED_OF_LIGHT;

        // 转换为大地坐标
        BLH blh = wgs84.XYZ2BLH(recPos);

        // 输出定位结果
        cout << fixed << setprecision(9);
        cout << "历元: " << epoch.time << endl;
        cout << "接收机位置 (ECEF): "
            << recPos.x << ", " << recPos.y << ", " << recPos.z << " m" << endl;
        cout << "接收机位置 (BLH): "
            << "纬度: " << blh.B << "°, "
            << "经度: " << blh.L << "°, "
            << "高程: " << blh.H << " m" << endl;
        cout << "接收机钟差: " << scientific << recClockBias << " s" << endl;
        cout << "----------------------------------------" << endl;
    }

    return 0;
}